/***************************************************************************
 创建者: 华磊
 开始时间: 2019.4.24
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:皮带跟踪
 *　１）主要是需要示教　触发点、Ａ点、B点，计算得到　３点关系、皮带方向、皮带传动比
 *                                                                         *
 ***************************************************************************/

#ifndef BELTTRACK_H
#define BELTTRACK_H

#include <QObject>
#include "globalDefine.h"
#include <QVector>
#include <QMap>
#include <QQueue>
#include <QMutex>
#include "MacroDefine/GeneralDefine.h"

#define D_TRANSITION_WAIT 0

class CoordinateManager;
class RegData;
class MotionMessage;
class QTcpSocket;
class VisionTrack;
class FilteringAlgorithm;

//enum E_MOVE_STAGE
//{
//    E_MOVE_STAGE_NONE,
//    E_MOVE_STAGE_GOMOVE_BEGIN,
//    E_MOVE_STAGE_MOVE_HOLDING,
//    E_MOVE_STAGE_STOPMOVE_BEGIN,

//};

enum E_MOVE_COMMAND
{
    E_MOVE_COMMAND_NONE,
    E_MOVE_COMMAND_GOMOVE,
    E_MOVE_COMMAND_STOPMOVE
};
#include "frames.hpp"
using namespace KDL;
class PositionMixerManager;

class BeltTrack : public QObject
{
    Q_OBJECT
public:
    explicit BeltTrack(int beltIndexIn, RegData *regDataIn, BeltTrackConfig configIn, MotionMessage *motionMessageIn,
                       CoordinateManager *coordinateManagerIn, double deltaTimeIn, QObject *parent = nullptr);

public:
    int setCurrentFrameCommand(Frame frameIn);
    int addCeaseMotion(Frame startFrameIn, Frame endFrameIn);
    int moveOneStep_FrameCommand(Frame &frameOut, double &durationLeftOut);
    double getBeltTransmission();
    int getTrackTransionDistance_start(double &deltaX,double &deltaY);
    int getTrackTransionDistance_end(double &deltaX,double &deltaY);
    int setDebugFlag(int flagIn);
    int recordLatchValue();
    int getEncoderDeviceType(int &deviceTypeOut, int &aliasOut, int &encoderIndexOut);
    int getDoType(int &doDeviceTypeOut,int &doIndexOut);
    int setTrackOffset(double offset_x,double offset_y);
    int addVisionObject(VisionObjectInfo &visionObjectIn);
    int getCurrentEncoderValue();
//    int getCurrentEncoderLatchValue();
    int reset();
    int cease();
//    int setTrackByVision(VisionTrack* trackIn);
    int setOriginalToolCoordinateIndex(int indexIn);
    int getBeltTrackDebugInfo(BeltTrackDebugInfo &beltTrackInfoOut);

    int modifyBeltTrackConfig(BeltTrackConfig configIn);

    //视觉点放入寄存器。根据筛选调节，设置当前跟踪点。
    int getTrackData();
    //设定当前用户坐标系号为D_TRACK_USER_COORDINATE_INDEX。切换瞬间轴的速度有冲击，需要处理下。
    int trackStart();
    int trackEnd();

    int loopCommontimeOnce();
    //（跟踪点的锁存编码器值和当前编码器值更新用户坐标系的值），实时更新用户坐标系
    /**
     * @brief loopRealtimeOnce
     * @param encoderData　编码器值
     * @param isTrajectoryEmptyStatusIn 轨迹是否为空
     * @param poseFeedback 单位ｍｍ
     * @param poseCommandOut 单位ｍｍ
     * @param isMoveValidOut　是否输出同步位置命令
     * @return
     */
    int loopRealtimeOnce(int &encoderData, PointPro &deltaPoseOut, E_MOVE_STAGE &stageOut);
    int getEncoderIndex();
    /**
     * @brief getActualMovedDistance 单位 米
     * @param oldValue
     * @param newValue
     * @return
     */
    double getActualMovedDistance(int oldValue,int newValue);
    int getActualMovedPulse(int oldValue,int newValue);


private:
    int testMovedDistance();
    int getBestPoint(QQueue<VisionObjectInfo> &visionPointBufferIn, VisionObjectInfo &objectOut);//todo 根据皮带运行方向进行排序．如果识别到多个物体，物体的顺序可能不合适．
    double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角
    int calculateMovedPoint(double xIn, double yIn, double movedLengthIn, double moveOrientation,
                            double &xOut, double &yOut);
    int initialByConfig();
    int deleteExceedPoint();

    int getQualifiedPointNum();

    int deleteExceedPoint_di();
    int deleteOverridePoint_di();
    int getQualifiedPointNum_di();

    int diSensorLoopLogic();
    int visionSensorLoopLogic();
    int moveTransitionLoop(PointPro &deltaPoseOut);

    E_MOVE_STAGE getMoveStage();

    double getActualMovedDistance(int latchValue);
    void addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode
                , int robotIdIn, int parameter1=0, int parameter2=0);

private:
    int beltIndex;
    bool isInitialOkStatus;
    QQueue<int> latchEncoderValueList;//锁存数值
    volatile bool isTracking;//是否正在跟踪
    CoordinateManager *coordinateManager;
    RegData *regData;
    MotionMessage *motionMessage;
    int robotId;

//    volatile unsigned short digitalOut;

    BeltTrackConfig beltConfig;
    volatile int loopCount;



    QMutex usingMutex;
    volatile int currentEncoderValue;
    volatile int currentEncoderValue_old;
    volatile int latchEncoderValue_old;
    volatile int currentEncoderLatchValue;
    volatile int latchValueForCoordinate;

    double validTrackStartPoint_fromA;
    double validTrackEndPoint_fromA;

    int originalUserCoordinateIndex;//用户设定的用户坐标系，固定的。D_TRACK_SET_USER_COORDINATE_INDEX跟踪时被激活，而且实时变化
    volatile E_MOVE_STAGE moveStage;
    volatile E_MOVE_COMMAND moveCommand;
    volatile double beltVelocity;//m/s
    double deltaTime;

    BeltTrackDebugInfo debugInfo;
    QQueue<VisionObjectInfo> visionPointBuffer;
    double beltMoveOrientation;//相对用户坐标系的皮带移动方向的角度
    int debugFlag;

    FilteringAlgorithm *filter_x;
    FilteringAlgorithm *filter_y;

    double acceleration_start;//mm/s^2
    double timeConsume_start;
    volatile double transionDistance_x_start;
    volatile double transionDistance_y_start;

    double acceleration_end;//mm/s^2
    double timeConsume_end;
    volatile double transionDistance_x_end;
    volatile double transionDistance_y_end;

    double refX_start,refY_start;
    double refX_end,refY_end;
    int velocityTick;
//    double startPose_x;//开始和结束过渡公用
//    double startPose_y;//开始和结束过渡公用
    double currentTransitionTimeAccumued;//开始和结束过渡公用

    PositionMixerManager *positionMixer;
    Frame *currentFrame;

};

#endif // BELTTRACK_H
